// pointcloud_mqtt_subscriber.cpp
#include "mqtt_cloud_sub/cloud_subscriber.hpp" // 包含更新后的头文件
#include <cstring>
#include <chrono>
#include <algorithm> // For std::min

// 定义删除器函数
void mosquitto_deleter(struct mosquitto* client) {
    if (client) {
        mosquitto_destroy(client);
    }
}

PointCloudMqttSubscriber::PointCloudMqttSubscriber()
    : rclcpp_lifecycle::LifecycleNode("pointcloud_mqtt_subscriber"),
      // 在构造函数中初始化 unique_ptr，传入自定义删除器
      mqtt_client_(nullptr, mosquitto_deleter)
{
    RCLCPP_INFO(this->get_logger(), "Lifecycle PointCloud MQTT Subscriber created.");
    // 初始化状态标志
    state_is_active_ = false;
    is_connected_ = false;
    stop_connect_thread_ = false;
    // MQTT 客户端指针在 on_configure 中初始化
}

// 必须定义析构函数
PointCloudMqttSubscriber::~PointCloudMqttSubscriber() {
    RCLCPP_INFO(this->get_logger(), "Lifecycle PointCloud MQTT Subscriber destroyed.");
    // 通常 unique_ptr 会自动清理资源，这里可以添加其他清理逻辑
    // 注意：析构函数中不应调用 mosquitto 相关函数，因为生命周期节点的生命周期管理器会处理
    // 确保在 on_cleanup 中已经停止了所有线程和 MQTT 客户端
    stop_connect_thread_ = true;
    if (connect_thread_.joinable())
    {
        connect_thread_.join();
    }
    // mqtt_client_ 会在其生命周期结束时自动调用 deleter
}

// --- 生命周期回调函数 - 在 .cpp 中定义，不使用 override ---
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
PointCloudMqttSubscriber::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
{
    RCLCPP_INFO(this->get_logger(), "Configuring...");

    try {
        // 1. 声明参数
        this->declare_parameter<std::string>("mqtt_net.host", "localhost");
        this->declare_parameter<int>("mqtt_net.port", 1883);
        this->declare_parameter<std::string>("ros.output_topic", "/mqtt_reconstructed_cloud");
        this->declare_parameter<std::string>("mqtt.data_topic", "/fangshe/robot/lidarb/cloud/data");
        this->declare_parameter<std::string>("mqtt.meta_topic", "/fangshe/robot/lidarb/cloud/meta");

        // 2. 获取参数值
        mqtt_host_ = this->get_parameter("mqtt_net.host").as_string();
        mqtt_port_ = this->get_parameter("mqtt_net.port").as_int();
        output_ros_topic_ = this->get_parameter("ros.output_topic").as_string();
        mqtt_data_topic_ = this->get_parameter("mqtt.data_topic").as_string();
        mqtt_meta_topic_ = this->get_parameter("mqtt.meta_topic").as_string();

        RCLCPP_INFO(this->get_logger(), "Output ROS Topic: %s", output_ros_topic_.c_str());
        RCLCPP_INFO(this->get_logger(), "MQTT Data Topic: %s", mqtt_data_topic_.c_str());
        RCLCPP_INFO(this->get_logger(), "MQTT Meta Topic: %s", mqtt_meta_topic_.c_str());

        // 3. 初始化 MQTT 客户端 (注意：必须先确保 mosquitto_lib_init() 已调用)
        // 由于在 main 函数中调用了 mosquitto_lib_init()，这里可以直接创建
        struct mosquitto *raw_client = mosquitto_new(nullptr, true, this);

        if (!raw_client)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to create MQTT client");
            return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
        }

        mosquitto_message_callback_set(raw_client, on_message_static);
        mosquitto_connect_callback_set(raw_client, on_connect_static);
        mosquitto_disconnect_callback_set(raw_client, on_disconnect_static);

        // 4. 将原始指针交给 unique_ptr 管理
        mqtt_client_.reset(raw_client);

        // 5. 创建 ROS 发布者 (但不激活)
        pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(output_ros_topic_, 10);

        RCLCPP_INFO(this->get_logger(), "Successfully configured.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Exception in on_configure: %s", e.what());
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
    }
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
PointCloudMqttSubscriber::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
{
    RCLCPP_INFO(this->get_logger(), "Activating...");

    try {
        if (!mqtt_client_) {
            RCLCPP_ERROR(this->get_logger(), "MQTT client is not initialized!");
            return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
        }

        // 1. 启动连接线程
        if (!connect_thread_.joinable()) {
            stop_connect_thread_ = false; // 确保标志为 false
            connect_thread_ = std::thread(&PointCloudMqttSubscriber::connect_loop, this);
        } else {
            RCLCPP_WARN(this->get_logger(), "Connect thread was already running during activation.");
        }

        // 2. 启动 MQTT loop
        int rc = mosquitto_loop_start(mqtt_client_.get());
        if (rc != 0)
        {
            RCLCPP_ERROR(this->get_logger(), "Could not start MQTT loop: %s", mosquitto_strerror(rc));
            mosquitto_disconnect(mqtt_client_.get());
            return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
        }

        // 3. 激活 ROS 发布者
        pointcloud_pub_->on_activate();

        state_is_active_ = true; // 标记为活动状态

        RCLCPP_INFO(this->get_logger(), "Successfully activated.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Exception in on_activate: %s", e.what());
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
    }
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
PointCloudMqttSubscriber::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
{
    RCLCPP_INFO(this->get_logger(), "Deactivating...");

    try {
        if (!mqtt_client_) {
            RCLCPP_ERROR(this->get_logger(), "MQTT client is not initialized!");
            return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
        }

        // 1. 停用 ROS 发布者
        pointcloud_pub_->on_deactivate();

        // 2. 停止 MQTT loop
        mosquitto_loop_stop(mqtt_client_.get(), true);

        // 3. 断开 MQTT 连接
        mosquitto_disconnect(mqtt_client_.get());

        // 4. 请求停止连接线程
        stop_connect_thread_ = true;

        // 5. 重置连接状态标志
        {
            std::lock_guard<std::mutex> lock(connect_mutex_);
            is_connected_ = false;
        }

        state_is_active_ = false; // 标记为非活动状态

        RCLCPP_INFO(this->get_logger(), "Successfully deactivated.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Exception in on_deactivate: %s", e.what());
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
    }
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
PointCloudMqttSubscriber::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/)
{
    RCLCPP_INFO(this->get_logger(), "Cleaning up...");

    try {
        // 1. 确保线程停止
        stop_connect_thread_ = true;
        if (connect_thread_.joinable())
        {
            connect_thread_.join();
        }

        // 2. 销毁 MQTT 客户端 (unique_ptr 会自动调用自定义删除器 mosquitto_deleter)
        mqtt_client_.reset(); // 这会调用 mosquitto_deleter

        // 3. 重置状态和缓存
        state_is_active_ = false;
        is_connected_ = false;
        cached_metadata_ = MetadataCache(); // 重置缓存

        RCLCPP_INFO(this->get_logger(), "Successfully cleaned up.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Exception in on_cleanup: %s", e.what());
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
    }
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
PointCloudMqttSubscriber::on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/)
{
    RCLCPP_INFO(this->get_logger(), "Shutting down...");
    // 在正常关机时，on_cleanup 通常会被调用，这里可以执行额外的清理
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
PointCloudMqttSubscriber::on_error(const rclcpp_lifecycle::State & /*previous_state*/)
{
    RCLCPP_ERROR(this->get_logger(), "Error occurred in lifecycle node!");
    // 可以在这里处理错误状态
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

// --- 原有回调函数定义 ---
void PointCloudMqttSubscriber::on_message(const struct mosquitto_message *msg)
{
    // 关键修改：如果节点未激活，则不处理或发布数据
    if (!state_is_active_) {
        // RCLCPP_DEBUG(this->get_logger(), "Node is not active, skipping MQTT message.");
        return;
    }

    std::string topic(msg->topic);

    if (topic == mqtt_meta_topic_)
    {
        RCLCPP_DEBUG(this->get_logger(), "Received metadata on [%s], size: %d", topic.c_str(), msg->payloadlen); // 修正格式符

        Json::Value root;
        Json::CharReaderBuilder reader_builder;
        std::string errors;
        std::unique_ptr<Json::CharReader> reader(reader_builder.newCharReader());
        std::string payload_str(static_cast<const char*>(msg->payload), msg->payloadlen);

        if (reader->parse(payload_str.c_str(), payload_str.c_str() + payload_str.length(), &root, &errors))
        {
            cached_metadata_.header.stamp.sec = root["header"]["stamp"]["sec"].asInt();
            cached_metadata_.header.stamp.nanosec = root["header"]["stamp"]["nanosec"].asUInt();
            cached_metadata_.header.frame_id = root["header"]["frame_id"].asString();
            cached_metadata_.height = root["height"].asUInt();
            cached_metadata_.width = root["width"].asUInt();
            cached_metadata_.is_bigendian = root["is_bigendian"].asBool();
            cached_metadata_.point_step = root["point_step"].asUInt();
            cached_metadata_.row_step = root["row_step"].asUInt();
            cached_metadata_.is_dense = root["is_dense"].asBool();

            cached_metadata_.fields.clear();
            const Json::Value& fields_json = root["fields"];
            for (const auto& field_json : fields_json)
            {
                sensor_msgs::msg::PointField field;
                field.name = field_json["name"].asString();
                field.offset = field_json["offset"].asUInt();
                field.datatype = static_cast<uint8_t>(field_json["datatype"].asInt());
                field.count = field_json["count"].asUInt();
                cached_metadata_.fields.push_back(field);
            }

            RCLCPP_DEBUG(this->get_logger(), "Cached metadata for frame_id: %s", cached_metadata_.header.frame_id.c_str());
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to parse metadata JSON: %s", errors.c_str());
        }
    }
    else if (topic == mqtt_data_topic_)
    {
        RCLCPP_INFO(this->get_logger(), "Received data on [%s], size: %d", topic.c_str(), msg->payloadlen); // 修正格式符

        if (cached_metadata_.header.frame_id.empty())
        {
            RCLCPP_WARN(this->get_logger(), "Received data but no cached metadata available. Dropping data packet.");
            return;
        }

        sensor_msgs::msg::PointCloud2::SharedPtr reconstructed_msg = std::make_shared<sensor_msgs::msg::PointCloud2>();

        reconstructed_msg->header = cached_metadata_.header;
        reconstructed_msg->height = cached_metadata_.height;
        reconstructed_msg->width = cached_metadata_.width;
        reconstructed_msg->fields = cached_metadata_.fields;
        reconstructed_msg->is_bigendian = cached_metadata_.is_bigendian;
        reconstructed_msg->point_step = cached_metadata_.point_step;
        reconstructed_msg->row_step = cached_metadata_.row_step;
        reconstructed_msg->is_dense = cached_metadata_.is_dense;

        reconstructed_msg->data.resize(msg->payloadlen);
        std::memcpy(reconstructed_msg->data.data(), msg->payload, msg->payloadlen);

        // 关键修改：如果发布者未激活，调用 publish 会失败或无效果，但检查状态是更好的实践
        // 由于我们在 on_deactivate 中调用了 on_deactivate()，所以这里的检查主要是保险
        if (pointcloud_pub_->is_activated()) {
             pointcloud_pub_->publish(*reconstructed_msg);
             RCLCPP_INFO(this->get_logger(), "Published reconstructed PointCloud2 to ROS topic: %s, size: %zu bytes",
                         output_ros_topic_.c_str(), reconstructed_msg->data.size());
        } else {
             RCLCPP_WARN(this->get_logger(), "Tried to publish but publisher is not activated.");
        }

        // 清除缓存的元数据 (取决于具体需求，这里选择不清除，假设元数据在一段时间内有效)
        // cached_metadata_ = MetadataCache();
    }
}

void PointCloudMqttSubscriber::on_connect(int rc)
{
    is_connected_ = (rc == 0); // 使用 atomic 赋值
    if (rc == 0)
    {
        RCLCPP_INFO(this->get_logger(), "Successfully connected to MQTT broker");

        if (state_is_active_) { // 只有在节点激活时才订阅
            resubscribe_topics();
        }
    }
    else
    {
        RCLCPP_ERROR(this->get_logger(), "Failed to connect to MQTT broker: %s", mosquitto_strerror(rc));
    }
}

void PointCloudMqttSubscriber::on_disconnect(int rc)
{
    is_connected_ = (rc == 0); // 使用 atomic 赋值
    if (rc != 0)
    {
        RCLCPP_WARN(this->get_logger(), "Disconnected from MQTT broker. Will attempt to reconnect...");
        // 在 on_deactivate 中会设置 stop_connect_thread_，所以重连只会在激活状态下发生
    }
}

// 静态回调函数定义
void PointCloudMqttSubscriber::on_message_static(struct mosquitto *mosq, void *obj, const struct mosquitto_message *msg)
{
    if (!obj || !msg) return;
    static_cast<PointCloudMqttSubscriber*>(obj)->on_message(msg);
}

void PointCloudMqttSubscriber::on_connect_static(struct mosquitto *mosq, void *obj, int rc)
{
    if (!obj) return;
    static_cast<PointCloudMqttSubscriber*>(obj)->on_connect(rc);
}

void PointCloudMqttSubscriber::on_disconnect_static(struct mosquitto *mosq, void *obj, int rc)
{
    if (!obj) return;
    static_cast<PointCloudMqttSubscriber*>(obj)->on_disconnect(rc);
}

void PointCloudMqttSubscriber::connect_loop()
{
    int retry_count = 0;
    const int max_retry_delay = 5;

    while (rclcpp::ok() && !stop_connect_thread_)
    {
        // 检查是否应该尝试连接 (仅在非连接状态且未被要求停止时)
        bool should_try_connect = !is_connected_ && !stop_connect_thread_;

        if (should_try_connect)
        {
            RCLCPP_INFO(this->get_logger(), "Trying to connect to MQTT broker: %s:%d",
                        mqtt_host_.c_str(), mqtt_port_);

            int rc = mosquitto_connect(mqtt_client_.get(), mqtt_host_.c_str(), mqtt_port_, 60);
            if (rc != 0)
            {
                int delay = std::min(1 << retry_count, max_retry_delay);
                RCLCPP_ERROR(this->get_logger(), "Could not connect to MQTT broker: %s. Retrying in %d seconds...",
                             mosquitto_strerror(rc), delay);
                std::this_thread::sleep_for(std::chrono::seconds(delay));
                retry_count++;
            }
            else
            {
                // 连接成功后，等待回调函数更新 is_connected_ 状态
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
            }
        }
        else
        {
            // 如果已经连接或被要求停止，则短暂休眠
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }
    RCLCPP_INFO(this->get_logger(), "MQTT connect loop ended.");
}

void PointCloudMqttSubscriber::resubscribe_topics()
{
    if (!mqtt_client_) {
        RCLCPP_ERROR(this->get_logger(), "MQTT client is not initialized during resubscribe!");
        return;
    }

    std::vector<std::string> topics = {mqtt_data_topic_, mqtt_meta_topic_};

    for (const auto &topic : topics)
    {
        int rc = mosquitto_subscribe(mqtt_client_.get(), nullptr, topic.c_str(), 0);
        if (rc != 0)
        {
            RCLCPP_ERROR(this->get_logger(), "Could not subscribe to MQTT topic [%s]: %s", topic.c_str(), mosquitto_strerror(rc));
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Subscribed to MQTT topic: %s", topic.c_str());
        }
    }
}

// 主函数
int main(int argc, char *argv[])
{
    // 在 main 函数中初始化 mosquitto 库
    mosquitto_lib_init();

    rclcpp::init(argc, argv);

    auto node = std::make_shared<PointCloudMqttSubscriber>();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node->get_node_base_interface());
    executor.spin();

    rclcpp::shutdown();
    // 在程序结束前清理 mosquitto 库
    mosquitto_lib_cleanup();
    return 0;
}